#include "parser.h"

Parser::Parser()
{
    for(int i=0; i<6; i++)
    {
        ball[i] = 0;

        line_r[i] = 0;
        line_l[i] = 0;

        flag_c[i]=0;
        flag_c_t [i]=0;
        flag_c_b [i]=0;

        flag_r_t [i]=0;
        flag_r_b [i]=0;

        flag_g_l_t[i]=0;
        goal_r[i]=0;
        flag_g_l_b[i]=0;

        flag_g_r_t[i]=0;
        goal_l[i]=0;
        flag_g_r_b[i]=0;

        p_id[0]=0;
        player[i]=0;
        team_name=" ";
    }
}

void Parser:: parse (string &data)
{
    //static int lasttime;
    if(strcmp(data.c_str(), "(see ")){

        findplayer(data, "((player ", p_id, team_name, player);

        findfloats(data, "(ball) ", ball);  //gets the ball's pos

        findfloats(data, "(line r) ", line_r);
        findfloats(data, "(line l) ", line_l);

        findfloats(data, "(goal r) ", goal_r);
        findfloats(data, "(goal l) ", goal_l);

        findfloats(data, "(flag c) ", flag_c);
        findfloats(data, "(flag c t) ", flag_c_t);
        findfloats(data, "(flag c b) ", flag_c_b);
        findfloats(data, "(flag r t) ", flag_r_t);
        findfloats(data, "(flag r b) ", flag_r_b);
        findfloats(data, "(flag g l t) ", flag_g_l_t);
        findfloats(data, "(flag g l b) ", flag_g_l_b);
        findfloats(data, "(flag g r t) ", flag_g_r_t);

        findint2(data, "(stamina ", stamina, effort);

        findfloats(data, "(speed ", speed);

        time=findint(data, "(see ");
    }

    if(strcmp(data.c_str(),"(body_sensor ")){

        head_angle=findint(data, "head_angle");

        counters.push_back(findint(data, "kick"));
        counters.push_back(findint(data, "dash"));
        counters.push_back(findint(data, "turn"));
        counters.push_back(findint(data, "say"));
        counters.push_back(findint(data, "turn_neck"));
        counters.push_back(findint(data, "catch"));
        counters.push_back(findint(data, "move"));
        counters.push_back(findint(data, "change_view"));


        time=findint(data, "(body_sensor ");
    }

    // cout << data << endl;
    //    cout << "Time: "<<time<<endl;
    //    cout << "Arg: " << ball[0] << "  Ang: "<<ball[1] << endl;
    //    cout << "Speed arg: " << speed[0] << "  Speed ang: " << speed[1] << endl;
    //    cout << "Stamina: " << stamina << "  Effort: " << effort << endl;
    //    cout << "Line r: " << line_r[0] << " Line l: " << line_l[0] << endl;
    //    cout << "Flag c: " << flag_c[0] <<  "  Flag c t: " << flag_c_t[0] << " Flag c b: " << flag_c_b[0] << endl;
    //    cout << "Flag g l t: " << flag_g_l_t[0] << " Flag g l b: " << flag_g_l_b[0] << endl;
    //    cout << "Flag g r t: " << flag_g_r_t[0] << " Flag g r b: " << flag_g_r_b[0] << endl;
    //    cout << "Goal r: " << goal_r[0]<<" " <<goal_r[1] << " Goal l: " << goal_l[0]<<" "<<goal_l[1]<< endl;
    cout << "Pid: " <<p_id[0] <<  "   Team:  " <<team_name << "  PLayer pos: " << player[0] << "  "<< player[1] << endl;
}

void Parser:: findfloats(string &str, const string name, float see_info[])
{
    size_t found =0, middle=0, final=0;
    int j=0, k=0;
    string Distance, Direction;// DistChange, DirChange, BodyFacingDir, HeadFacingDir;

    if (name=="(ball) ")
        objectball=true;

    found=str.find(name);

    if (strcmp(str.c_str(), name.c_str()) && found!= string::npos)
    {
        Direction.clear();
        Distance.clear();

        found+=name.length();
        middle=str.find(" ", found);
        final=str.find(")", middle);

        for (size_t i= found; i <= final; i++)
        {
            if (i<middle){
                Distance[j]=str.at(i);
                j++;
            }
            if (i==middle)
            {
                Distance[j]= '\0';
                see_info[0]= atof(Distance.c_str());

            }

            if (i>middle && i<final){
                Direction[k]=str.at(i);
                k++;
            }

            if (i==final)
            {
                Direction[k]= '\0';
                see_info[1]= atof(Direction.c_str());
            }
        }
    }
}


void Parser:: findplayer(string &str, const string name, float id[], string &team_name, float see_info[])
{
    size_t found =0, id_t=0, n1=0, n2=0,n3=0,n4=0, final=0;
    int j=0;
    string id_number, team, Distance, Direction, DistChange, DirChange;// BodyFacingDir, HeadFacingDir;

    found=str.find(name);

    if (strcmp(str.c_str(), name.c_str()) && found!= string::npos)
    {
        Direction.clear();
        Distance.clear();
        DistChange.clear();
        DirChange.clear();
        team.clear();

        found+=name.length();
        id_t=str.find(" ", found);
        n1=str.find(") ", id_t);
        n2=str.find(" ", n1+2);
        n3=str.find(" ", n2+1);
        n4=str.find(" ", n3+1);
        final=str.find(")", n4+1);

        cout << "posiciones: " << found << " " << id_t << " " << n1 << " "<< n2 <<" "<<n3 << " " <<n4 << " " << final<< endl;

        for (size_t i= found; i <= final; i++)
        {
            //team name
            if (i<id_t)
            {
                team[j]=str.at(i);
                j++;
            }
            if (i==id_t)
            {
                team[j]= '\0';
                team_name= team.c_str();
                //cout << " team: " << team_name << endl;

            }
            j=0;

            //id number
            if (i<n1)
            {
                id_number[j]=str.at(i);
                j++;
            }

            if (i==n1)
            {
                id_number[j]='\0';
                id[0] = atof(id_number.c_str());
                //cout << " id: " << id_number.c_str() << endl;
            }
            j=0;

            //Distance
            if (i<n2)
            {
                Distance[j]=str.at(i);
                j++;
            }

            if (i==n2)
            {
                Distance[j]= '\0';
                see_info[0]= atoi(Distance.c_str());
                 //cout << " pos: " << Distance.c_str() << endl;
            }
            j=0;

            //Direction
            if (i<n3)
            {
                Direction[j]=str.at(i);
                j++;
            }

            if (i==n3)
            {
                Direction[j]= '\0';
                see_info[1]= atoi(Direction.c_str());
                //cout << " v: " << see_info[1] << endl;
            }
        }
    }
}


int  Parser:: findint(string &str, const string name)
{
    size_t found=0;
    int j=0;
    string solution, lastsolution;

    found=str.find(name);

    if (found!= string::npos)
    {
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (&str[i]==" ")
                break;
            solution[j]=str.at(i);
            j++;
        }
    }
    else
        solution=lastsolution;

    lastsolution=solution;
    return atoi(solution.c_str());
}


void  Parser:: findint2(string &str, const string name, unsigned int &value1, unsigned int &value2)
{
    size_t found=0;
    int j=0, n=0;
    string solution=" ";

    found=str.find(name);

    if (found!= string::npos)
    {
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (str.at(i) == ' ')
                break;
            solution[j]=str.at(i);
            j++;
            n=(int)i;
        }

        value1= atoi(solution.c_str());
        j=0;
        n=n+2;

        if (str.at(n)!= ')')
        {
            solution=" ";

            for (size_t k= n; k < str.length(); k++)
            {
                if (str.at(k) == ')')
                    break;
                solution[j]=str.at(k);
                j++;

            }
            value2= atoi(solution.c_str());
        }
    }
}


void  Parser:: findint2(string &str, const string name, int value[]) //sobrecarga
{
    size_t found=0;
    int j=0, n=0;
    string solution=" ";

    found=str.find(name);

    if (found!= string::npos)
    {
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (str.at(i) == ' ')
                break;
            solution[j]=str.at(i);
            j++;
            n=(int)i;
        }

        value[0]= atoi(solution.c_str());
        j=0;
        n=n+2;

        if (str.at(n)!= ')')
        {
            solution=" ";

            for (size_t k= n; k < str.length(); k++)
            {
                if (str.at(k) == ')')
                    break;
                solution[j]=str.at(k);
                j++;

            }
            value[1]= atoi(solution.c_str());
        }
    }
}



string  Parser:: findstring(string &str, const string name)
{
    size_t found;
    int j=0;
    string solution;

    if (str.compare(name)){
        found=str.find(name);
        for (size_t i= found+name.length(); i < str.length(); i++)
        {
            if (&str[i]==" ")
                break;
            solution[j++]=str.at(i);

        }
        return (solution);
    }
}



